#pragma once

#include <fstream>
#include <vector>

#include "data_storage.hpp"

class TruthFileLoader {
private:
    std::ifstream m_truth_data_file;

public:
    TruthFileLoader(std::string filename, double timestamp = -1);
    ~TruthFileLoader() {}

    std::vector<iNav::Truth> m_truth_vec;
};

TruthFileLoader::TruthFileLoader(std::string filename, double timestamp)
    : m_truth_data_file(filename.c_str()) {
    while (!m_truth_data_file.eof()) {
        iNav::Truth truth_frame;
        m_truth_data_file >> truth_frame.week >> truth_frame.second >> truth_frame.pos[0] >>
            truth_frame.pos[1] >> truth_frame.pos[2] >> truth_frame.vel[0] >> truth_frame.vel[1] >>
            truth_frame.vel[2] >> truth_frame.att[0] >> truth_frame.att[1] >> truth_frame.att[2] >>
            truth_frame.angular[0] >> truth_frame.angular[1] >> truth_frame.angular[2];
        if (truth_frame.second >= timestamp)
            m_truth_vec.push_back(truth_frame);
    }
}